#include "ctprl.h"

static unsigned char getMask(int length)
{
	switch(length)
	{
	case 8:
		return 0xff;
	case 7:
		return 0xfe;
	case 6:
		return 0xfc;
	case 5:
		return 0xf8;
	case 4:
		return 0xf0;
	case 3:
		return 0xe0;
	case 2:
		return 0xC0;
	case 1:
		return 0x80;
	default:
		return 0x0;

	}
}

int getBit(unsigned char* pData, int pos)
{
	unsigned char cTemp;
	int segment=0;
	int offset=0;

	segment = pos / 8;
	offset  = pos % 8;

	cTemp = pData[segment];
	cTemp >>= offset;
	cTemp = cTemp & 1;

	return (int)cTemp;
}

void setBit(unsigned char* pData, int pos, int value)
{
	unsigned char cTemp;
	int segment=0;
	int offset=0;

	segment = pos / 8;
	offset  = pos % 8;

	cTemp = value & 0x1;
	cTemp <<= (7-offset);
	
	pData[segment] |= cTemp;
	
}

/**
 * For the bits data, usually the padding bits are in the tail. 
 * In other words, the bits are left align. 
 */

int getBits(unsigned char* pData, int start, int length, unsigned char* pOut)
{
	int startseg = start / 8;
	int startoff = start % 8;
	int endseg = (start+length-1) / 8;
	int endoff = (start+length-1) % 8;

	if(startseg == endseg){
		pOut[0] = pData[startseg] << startoff;
		pOut[0] &= getMask(length);
		return 1;
	}
	
	for(int i = 0; i<endseg-startseg; i++)
	{
		pOut[i] = pData[startseg+i] << startoff;
		pOut[i] |= (pData[startseg+i+1] >> (8- startoff));
	}

	pOut[endseg-startseg] = pData[endseg] << startoff;
	pOut[endseg-startseg] &= getMask(length % 8);

	return (endseg-startseg+1);
}

int getIntFromBits(unsigned char* pData, int start, int length)
{
	unsigned char pTemp[4];
	
	if(length>32 || length<0)
	{
		return 0;
	}

	for(int i=0;i<4;i++)
	{
		pTemp[i] = 0;
	}

	int length2 = getBits(pData,start,length,pTemp);

	int iTemp = 0;

	if(length2 ==1 ){
		iTemp = pTemp[0];
	}
	else if(length2 ==2)
	{
		iTemp = (pTemp[0] <<8) +pTemp[1];
	}
	else if(length2 ==3)
	{
		iTemp = (pTemp[0] <<16) + (pTemp[1]<<8)+ pTemp[0];
	}
	else
	{
		int iTemp = (pTemp[0]<<24) + (pTemp[1]<<16)+ (pTemp[2]<<8) + pTemp[3];
	}

	iTemp >>= ((length % 8)==0? 0 : 8-(length%8));

	return iTemp;
}

void parsePRL(unsigned char* pData, int length)
{

	int iPos=0;

#define READU2 readU2(pData,&iPos)

	int prlLength = READU2;
	printf("The PRL length is %d\n",prlLength);

	int prlID = READU2;
	printf("The PRL ID is %x\n",prlID);

	int iPrefOnly = getIntFromBits(pData+iPos,0,1);
	printf("Pref Only = %s\n",(iPrefOnly? "True":"False"));

	int iDefRoamInd = getIntFromBits(pData+iPos,1,8);
	printf("Default Roaming Indicator = %d\n",iDefRoamInd);

	int iNumAcqRecs = getIntFromBits(pData+iPos,9,9);
	printf("Number of Acquisition records: %d\n",iNumAcqRecs);

	int iNumSysRecs = getIntFromBits(pData+iPos,18,14);
	printf("Number of System records: %d\n",iNumSysRecs);

#undef READU2
}

void execCommand(unsigned char* pCommand, int length)
{
}

void update(int position, unsigned char* pData, int length)
{
}

void moveBytes(int forwardStep, unsigned char* pData,int startPos, int length)
{
}

unsigned char moveBits(int forwardStep, unsigned char* pData,int startPos, int startBit,int length)
{
	unsigned char cTemp=0;
	unsigned char cTemp2 = 0;

/*	if(forwardStep>0)
	{
		cTemp = pData[startPos]>>();
	}
	else
	{
	}*/

	return cTemp2;
}

void commit(unsigned char* pData, FILE* fp)
{
}
